package dfsketches.pathfind;

import java.util.Arrays;
import java.util.TreeSet;

import dfsketches.bicycles.BicycleStack;

public class AdvancedPathfind {
	private class Pair implements Comparable<Pair> {

		int x;
		int y;
		float dist;
		
		Pair(int x, int y, float dist) {
			this.x = x;
			this.y = y;
			this.dist = dist;
		}
		
		@Override
		public int compareTo(Pair o) {
			return (dist != o.dist) ? (int)Math.ceil(dist - o.dist) : 
				(x != o.x) ? x - o.x : y - o.y;
		}
		
		public boolean equals(Object o) {
			return (o instanceof Pair && compareTo((Pair)o) == 0);
		}
		
		@Override
		public String toString() {
			return "(" + x + ", " + y + ")" + " - " + dist;
		}
		
	}
	
	//I suppose values are being returned by PathSolver to be greater than zero.
	//Комментарий: полагаю, что PathSolver возвращает строго положительные значения.
	public BicycleStack find(BitMap map, int x_s, int y_s, int x_f, int y_f) {
		int w = map.width();
		int h = map.height();
		float[][] dist = new float[w][h];
		for (int i = 0; i < w; i++)
			Arrays.fill(dist[i], Float.MAX_VALUE);

		int[] deltaX = {0,
				-1,0,1,
				-1,  1,
				-1,0,1
		};
		int[] deltaY = {0,
				-1,-1,-1,
				 0,    0,
				 1, 1, 1
		};
		
		TreeSet<Pair> que = new TreeSet<Pair>();
		dist[x_s][y_s] = 0;
		que.add(new Pair(x_s, y_s, 0));
		while (!que.isEmpty()) {
			Pair p = que.pollFirst();
			for (int i = 1; i < 9; i++) {
				int x = p.x + deltaX[i];
				int y = p.y + deltaY[i];
				
				if (x < 0 || w <= x || y < 0 || h <= y)
					continue;
				
				float d = p.dist + 1;								//TODO: use PathSolver here
				if (!map.isWall(x, y)) {							//      ... and here
					if (dist[x][y] > d) {
						Pair np = new Pair(x, y, dist[x][y]);
						if (dist[x][y] != Float.MAX_VALUE)
							que.remove(np);
						np.dist = dist[x][y] = d;
						que.add(np);
					}
				}
			}
		}
		
		if (dist[x_f][y_f] == Float.MAX_VALUE) return null;

		BicycleStack path = new BicycleStack(0);
		int x = x_f;
		int y = y_f;
		float d = dist[x_f][y_f];
		while (d > 0) {
			path.push(x*h + y);
			for (int i = 1; i < 9; i++) {
				int nx = x + deltaX[i];
				int ny = y + deltaY[i];
				if (nx < 0 || w <= nx || ny < 0 || h <= ny)
					continue;
				if (dist[nx][ny] < d) {
					x = nx;
					y = ny;
					d = dist[x][y];
					break;	//for
				}
			}
		}
		return path;
	}
	
}